#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <errno.h>
#include <fcntl.h>

static int serialFd;

int openPort(void)
{
	int fd = open("/dev/ttyS0", O_RDWR | O_NOCTTY | O_NDELAY);

	if(fd >= 0) {
		fcntl(fd, F_SETFL, 0);
		struct termios options;
		tcgetattr(fd, &options);

		options.c_cflag = CLOCAL | CREAD | CS8;
		options.c_iflag = IGNBRK | IGNPAR;
		options.c_oflag = 0;
		options.c_lflag = 0;

		cfsetispeed(&options, B57600);
		cfsetospeed(&options, B57600);

		tcsetattr(fd, TCSANOW, &options);

		serialFd = fd;
		return fd;
	}

	perror("Failed to openport /dev/ttyS0 - ");
}

void oiStart(void)
{
	write(serialFd, "\x80", 1);
	usleep(15000);
}

void setSafeMode(void)
{
	write(serialFd, "\x83", 1);
	usleep(15000);
}

// go forward using opcode 152 run script to run set distance d in mm
// at velocity v in mm/sec this cannot be stopped or interupted
void forward(short v, short d)
{
	char cmd[] = {152,13,  		//init script
		137,0,0,0x7f,0xff,	//set motor speed
		156,0,0,       		//wait until traveled d
		137,0,0,0,0};  		//stop motors

	cmd[3] = v&0xff00 >> 8;
	cmd[4] = v&0xff;

	cmd[8] = d&0xff00 >> 8;
	cmd[9] = d&0xff;

	write(serialFd, cmd, sizeof(cmd));
	usleep(15000);
}

// turn angle in degrees at motor speed v
// clockwise is negative
void turnAngle(short v, short angle)
{
	short vl = v;
	short vr = -v;

	char cmd[] = {152,13,		// init script
		145,0,0,0,0,		// set motor speed
		157,0,0,		// wait angle
		145,0,0,0,0};		// stop motors

	cmd[3] = vr&0xff00 >> 8;
	cmd[4] = vr&0xff;

	cmd[5] = vl&0xff00 >> 8;
	cmd[6] = vl&0xff;

	cmd[8] = angle&0xff00 >> 8;
	cmd[9] = angle&0xff;

	write(serialFd, cmd, sizeof(cmd));
	usleep(15000);
}

void setDrive(short vl, short vr)
{
	char cmd[] = {145,0,0,0,0};

	cmd[1] = vr&0xff00 >> 8;
	cmd[2] = vr&0xff;

	cmd[3] = vl&0xff00 >> 8;
	cmd[4] = vl&0xff;

	write(serialFd, cmd, sizeof(cmd));
	usleep(15000);
}

int hitWall(void)
{
	char cmd[] = {142,7};
	char buffer;

	write(serialFd, cmd, sizeof(cmd));
	read(serialFd, &buffer, 1);

	usleep(15000);
	return buffer;
}

// return distance robot has traveled in mm since last querried
short readDist(void)
{
	short d = 0;
	char cmd[] = {142,19}; // request distance packet
	char buffer[2];

	write(serialFd, cmd,sizeof(cmd));
	read(serialFd, buffer, 2);

	d |= buffer[0] << 8 | buffer[1];

	usleep(15000);
	return d;
}

// return angle the robot has turned in degrees since last querried
short readAngle(void)
{
	short a = 0;
	char cmd[] = {142,20}; // request angle packet
	char buffer[2];

	write(serialFd, cmd,sizeof(cmd));
	read(serialFd, buffer, 2);

	a |= buffer[0] << 8 | buffer[1];

	usleep(15000);
	return a;
}
